#include "lowpass_filter.h"

void LowPassFilter_Init(LowPassFilter_t *handle,float Tf)
{
    handle->timestamp_prev = Simple_micros();
}


float LowPassFilter_GetValue(LowPassFilter_t *handle,float x)
{
    unsigned long timestamp = Simple_micros();
    float dt = (timestamp - handle->timestamp_prev)*1e-6f;

    if (dt < 0.0f ) dt = 1e-3f;
    else if(dt > 0.3f) {
        handle->y_prev = x;
        handle->timestamp_prev = timestamp;
        return x;
    }

    float alpha = handle->Tf/(handle->Tf + dt);
    float y = alpha*handle->y_prev + (1.0f - alpha)*x;
    handle->y_prev = y;
    handle->timestamp_prev = timestamp;
    return y;
}
